#include "./handler_task.h"
#include "../pub_service/ota_service.h"
#include "../pub_driver/cmd_can.h"
#include "../pearl_driver/move_motor.h"
#include "../pearl_driver/updown_motor.h"
#include "../pearl_driver/motors.h"
#include "../pearl_driver/sensor.h"

namespace pearl_task {

using namespace pub_service;
using namespace pub_driver;
using namespace pearl_driver;
using namespace conn;

HandlerTask handlerTask;

void HandlerTask::init_start() {
    m_mq.init();
    start();
}

static void on_move_motor_ctrl(const Frame &frame) {
    uint16_t mm;
    Error err = frame.parse(mm);
    if (err) {
        goto finish;
    }

    if (!sensor->is_updown_origin()) {
        err = ec::pearl::UPDOWN_IS_NOT_ORIGIN;
        goto finish;
    }

    if (!sensor->is_dump_bottom()) {
        err = ec::pearl::DUMP_IS_NOT_ORIGIN;
        goto finish;
    }
    
    err = moveMotor.run_with_mm(mm);
finish:
    cmdCAN.fast_res(frame, err);
}

static void on_updown_motor_ctrl(const Frame &frame) {
    uint16_t mm;
    Error err = frame.parse(mm);
    if (err) {
        goto finish;
    }
    
    err = updownMotor.run_with_mm(mm);
finish:
    cmdCAN.fast_res(frame, err);
}

static void on_dump_ctrl(const Frame &frame) {
    uint8_t val;
    Error err = frame.parse(val);
    if (err) {
        goto finish;
    }   

    if (!sensor->is_updown_origin()) {
        err = ec::pearl::UPDOWN_IS_NOT_ORIGIN;
        goto finish;
    }

    switch (val) {
        case 0: err = motors.dump_bottom(); break;
        case 1: err = motors.dump_top(); break;
        default: err = ec::pub::INVALID; break;
    }
finish:
    cmdCAN.fast_res(frame, err);
}

void HandlerTask::run() {

    Frame frame;
    for (;;) {
        m_mq.poll(frame);
        task_info.seq = frame.seq_cmd();
        task_info.cmd = frame.cmd();

        switch (frame.cmd()) {
            case proto::pearl::DUMP_CTRL: on_dump_ctrl(frame); break;
            case proto::pearl::UPDOWN_CTRL: on_updown_motor_ctrl(frame); break;
            case proto::pearl::MOVE_CTRL: on_move_motor_ctrl(frame); break;
            case proto::pub::OTA_START: pub_service::ota_start(frame); break;
            case proto::pub::OTA_TRANSLATE: pub_service::ota_translate(frame); break;
            case proto::pub::OTA_COMPLETE: pub_service::ota_complete(frame); break;
        }
    
        task_info.reset();
    }
}

}

